using System;
using BrainTechLLC.ThreadSafeObjects;
using BrainTechLLC;
using System.Collections.Generic;
using System.Text;

namespace AI
{
	public enum SB
	{
		None = 0,
		Seek,
		Flee,
		Arrive,
		Pursuit,
		Evade,
		Wander,
		PathFollowing,
		Cohesion,
		Alignment,
		Separation,
		CF,
		FCAS,
		FCS,
		CS,
		CA,
		CAS
	};

	//these define the force generated; represendted by a 2D vector
	public class SteeringBehaviours
	{
		private readonly static Vector4BaseFloat EmptyVect = new Vector4BaseFloat();
		private static Random random = new Random();

		public static Vector4BaseFloat None()
		{
			return EmptyVect;
		}

		//The seek steering behavior returns a force that directs an agent toward a target position
		public static Vector4BaseFloat Seek(Vector4BaseFloat targetPosition, Vector4BaseFloat currentPosition, ISpeedFloat Velocity, float max_linear_speed)
		{
			Vector4BaseFloat desired_V = Vector4BaseFloat.Normalize2D(Vector4BaseFloat.Subtract(targetPosition, currentPosition)) * max_linear_speed;
			return Vector4BaseFloat.Subtract(desired_V, Velocity as Vector4BaseFloat);
		}

		//Flee is the opposite of seek. Instead of producing a steering force to steer the agent toward a target position, flee creates a force that steers the agent away. 
		public static Vector4BaseFloat Flee(Vector4BaseFloat targetPosition, Vector4BaseFloat currentPosition, ISpeedFloat Velocity, Vector4BaseFloat desiredPosition, float max_linear_speed, int FOV, int vehicleNo)
		{
			if (Vector4BaseFloat.Length2D(Vector4BaseFloat.Subtract(targetPosition, currentPosition)) > FOV)
			{
				return None();
			}
			else
			{
				// CHECK this - it might be wrong
				Vector4BaseFloat desired_V = Vector4BaseFloat.Normalize2D(Vector4BaseFloat.Subtract(currentPosition, desiredPosition)) * max_linear_speed;
				return Vector4BaseFloat.Subtract(desired_V, Velocity as Vector4BaseFloat);
			}
		}

		//Seek is useful for getting an agent moving in the right direction, but often you'll want your agents
		//to come to a gentle halt at the target position, and as you've seen, seek is not too great at stopping gracefully.
		//Arrive is a behavior that steers the agent in such a way it decelerates onto the target position.
		public static Vector4BaseFloat Arrive(Vector4BaseFloat targetPosition, Vector4BaseFloat currentPosition, ISpeedFloat Velocity, int arriveRadius, float max_linear_speed, int vehicleNo)
		{
			Vector4BaseFloat toTarget = Vector4BaseFloat.Subtract(targetPosition, currentPosition);
			float distance = toTarget.Length2D();
			if (distance > 0)
			{
				double speed = max_linear_speed * (distance / arriveRadius);
				speed = Math.Min(speed, max_linear_speed);
				Vector4BaseFloat desired_V = toTarget * (float)(speed / distance);
				return Vector4BaseFloat.Subtract(desired_V, Velocity as Vector4BaseFloat);
			}
			return None();
		}

		//You'll often find wander a useful ingredient when creating an agent's behavior.
		//It's designed to produce a steering force that will give the impression of a random walk through the agent's environment.
		public static Vector4BaseFloat Wander(Vector4BaseFloat wanderTarget, Vector4BaseFloat currentPosition, ISpeedFloat Velocity, Vector4BaseFloat heading, float wanderRadius, float wanderDistance, int wanderJitter)
		{
			heading = Vector4BaseFloat.Normalize2D(Velocity as Vector4BaseFloat);
			wanderTarget += new Vector4BaseFloat(random.Next(-wanderJitter, wanderJitter), random.Next(-wanderJitter, wanderJitter), currentPosition.Z, currentPosition.W);
			wanderTarget = Vector4BaseFloat.Normalize2D(wanderTarget);
			wanderTarget *= wanderRadius / 2;
			Vector4BaseFloat circleCenterM = new Vector4BaseFloat((heading.X * wanderDistance) + currentPosition.X, (heading.Y * wanderDistance) + currentPosition.Y, 0);
			Vector4BaseFloat pointOnCircle = new Vector4BaseFloat(circleCenterM.X + wanderTarget.X, circleCenterM.Y + wanderTarget.Y, 0);
			return Vector4BaseFloat.Subtract(pointOnCircle, currentPosition);

		}

		//Path following creates a steering force that moves a vehicle along a series of waypoints forming a path.
		//Sometimes paths have a start and end point, and other times they loop back around on themselves forming a never-ending, closed path. 
		public static Vector4BaseFloat PathFollowing(Vector4BaseFloat targetPosition, Vector4BaseFloat currentPosition, ISpeedFloat Velocity, Vector4BaseFloat[] pathPoints, int currentPathPoint, int maxPathPoints, float max_linear_speed)
		{
			if (currentPathPoint > maxPathPoints - 1)
			{
				currentPathPoint = 0;
			}
			int nextPathPoint = currentPathPoint + 1;
			if ((nextPathPoint > (maxPathPoints - 1)))
			{
				nextPathPoint = 0;
			}
			Vector4BaseFloat currentPath = new Vector4BaseFloat(pathPoints[currentPathPoint]);
			Vector4BaseFloat differenceV = Vector4BaseFloat.Subtract(currentPosition, currentPath);
			if (Math.Abs(differenceV.Length2D()) < 5)
			{
				targetPosition = new Vector4BaseFloat(pathPoints[nextPathPoint].X, pathPoints[nextPathPoint].Y, currentPosition.Z, currentPosition.W);
				++currentPathPoint;
				return Seek(targetPosition, currentPosition, Velocity, max_linear_speed);
			}
			else
			{
				targetPosition = new Vector4BaseFloat(pathPoints[currentPathPoint]);
				//currentPathPoint++;
				return Seek(targetPosition, currentPosition, Velocity, max_linear_speed);
			}
		}

		//Cohesion produces a steering force that moves a vehicle toward the center of mass of its neighbors
		//A sheep running after its flock is demonstrating cohesive behavior. Use this force to keep a group of vehicles together.
		public static Vector4BaseFloat Cohesion(IEnumerable<IHasLocationFloat> allCars, IHasLocationFloat me, Vector4BaseFloat currentPosition, ISpeedFloat velocity, float max_linear_speed, int cohesionRadius)
		{
			int j = 0;
			Vector4BaseFloat averagePosition = new Vector4BaseFloat();
			Vector4BaseFloat distance = new Vector4BaseFloat();
			foreach (IHasLocationFloat loc in allCars)
			{
				distance = Vector4BaseFloat.Subtract(currentPosition, loc.Location as Vector4BaseFloat);
				if (Vector4BaseFloat.Length2D(distance) < cohesionRadius && loc != me)
				{
					j++;
					averagePosition = Vector4BaseFloat.Add(averagePosition, loc.Location as Vector4BaseFloat);
					//averagePosition = Vector4BaseFloat.Multiply(averagePosition, 10);
					averagePosition = Vector4BaseFloat.Add(averagePosition, Vector4BaseFloat.Normalize2D(distance) / Vector4BaseFloat.Length2D(distance));
				}
			}
			if (j == 0)
			{
				return None();
			}
			else
			{
				averagePosition = averagePosition / j;
				return Seek(averagePosition, currentPosition, velocity, max_linear_speed);
			}
		}

		//Alignment attempts to keep a vehicle's heading aligned with its neighbors
		//The force is calculated by first iterating through all the neighbors and averaging their heading vectors. 
		//This value is the desired heading, so we just subtract the vehicle's heading to get the steering force.
		public static Vector4BaseFloat Alignment(IEnumerable<IHasLocationFloat> allCars, IHasLocationFloat me, Vector4BaseFloat currentPosition, ISpeedFloat velocity, float max_linear_speed)
		{
			int j = 0;
			Vector4BaseFloat averageDirection = new Vector4BaseFloat();
			Vector4BaseFloat distance = new Vector4BaseFloat();
			foreach (IHasLocationFloat loc in allCars)
			{
				distance = Vector4BaseFloat.Subtract(currentPosition, loc.Location as Vector4BaseFloat);
				if (Vector4BaseFloat.Length2D(distance) < 100 && loc != me)
				{
					j++;
					averageDirection = Vector4BaseFloat.Add(averageDirection, velocity as Vector4BaseFloat);
				}
			}
			if (j == 0)
			{
				return None();
			}
			else
			{
				averageDirection = averageDirection / j;
				return Vector4BaseFloat.Subtract(averageDirection, velocity as Vector4BaseFloat);
			}
		}

		//Separation creates a force that steers a vehicle away from those in its neighborhood region. 
		//When applied to a number of vehicles, they will spread out, trying to maximize their distance from every other vehicle
		public static Vector4BaseFloat Separation(IEnumerable<IHasLocationFloat> allCars, IHasLocationFloat me, Vector4BaseFloat currentPosition, ISpeedFloat velocity, float max_linear_speed)
		{
			int j = 0;
			Vector4BaseFloat separationForce = new Vector4BaseFloat();
			Vector4BaseFloat averageDirection = new Vector4BaseFloat();
			Vector4BaseFloat distance = new Vector4BaseFloat();
			foreach (IHasLocationFloat loc in allCars)
			{
				distance = Vector4BaseFloat.Subtract(currentPosition, loc.Location as Vector4BaseFloat);
				if (Vector4BaseFloat.Length2D(distance) < 100 && loc != me)
				{
					j++;
					separationForce += Vector4BaseFloat.Subtract(currentPosition, loc.Location as Vector4BaseFloat);
					separationForce = Vector4BaseFloat.Normalize2D(separationForce);
					separationForce = Vector4BaseFloat.Multiply(separationForce, 1 / .7f);
					averageDirection = Vector4BaseFloat.Add(averageDirection, separationForce);
				}
			}
			if (j == 0)
			{
				return None();
			}
			else
			{
				//averageDirection = averageDirection / j;
				return averageDirection;
			}
		}

		public readonly static int CohesionRadius = 100;
		public readonly static int FOV = 100;    //field of view
		public readonly static float WanderRadius = 40, WanderDistance = 80;
		public readonly static int WanderJitter = 10;
		public readonly static int ArriveRadius = 100;

		//calculate force and update position
		public PositionAndSpeedFloat UpdatePosition(IPositionAndSpeedFloat posAndSpeed, float mass, float max_linear_speed, float max_force, SB sb, Vector4BaseFloat targetPosition, Vector4BaseFloat[] pathPoints, int currentPathPoint, IEnumerable<IHasLocationFloat> nearby)
		{
			PositionAndSpeedFloat newPS = new PositionAndSpeedFloat();
			Vector4BaseFloat steerForce = new Vector4BaseFloat();
			Vector4BaseFloat acceleration;
			Vector4BaseFloat currentPosition = posAndSpeed.Location as Vector4BaseFloat;
			Vector4BaseFloat currentDirection = posAndSpeed.DirectionVector as Vector4BaseFloat;
			ISpeedFloat velocity = posAndSpeed.SpeedVector;

			Vector4BaseFloat newDirection = (posAndSpeed.SpeedVector as Vector4BaseFloat).Normalize3D();
			newPS.DirectionVector.Set(newDirection);
			switch (sb)
			{
				case SB.None:
					steerForce = SteeringBehaviours.None();
					break;
				case SB.Seek:
					steerForce = SteeringBehaviours.Seek(targetPosition, currentPosition, velocity, max_linear_speed);
					break;
				case SB.Flee:
					steerForce = SteeringBehaviours.Flee(targetPosition, currentPosition, velocity, targetPosition, max_linear_speed, FOV, 1);
					break;
				case SB.Arrive:
					steerForce = SteeringBehaviours.Arrive(targetPosition, currentPosition, velocity, ArriveRadius, max_linear_speed, 1);
					break;
				case SB.Pursuit:

					break;
				case SB.Evade:
					break;
				case SB.Wander:
					// need to pass back to newDirection?
					steerForce = SteeringBehaviours.Wander(targetPosition, currentPosition, velocity, newDirection, WanderRadius, WanderDistance, WanderJitter);
					break;
				case SB.PathFollowing:
					steerForce = SteeringBehaviours.PathFollowing(targetPosition, currentPosition, velocity, pathPoints, currentPathPoint, pathPoints.Length, max_linear_speed);
					break;
				case SB.Cohesion:
					steerForce = SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius);
					break;
				case SB.Alignment:
					steerForce = SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed);
					break;
				case SB.Separation:
					steerForce = SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed);
					break;
				case SB.CF:
					steerForce = Vector4BaseFloat.Add(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), SteeringBehaviours.Flee(targetPosition, currentPosition, velocity, targetPosition, max_linear_speed, FOV, 1));
					//steerForce = Vector4BaseFloat.Add(SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), Vector4BaseFloat.Add(SteeringBehaviours.Separation(nearby, this, currentPosition, velocity, max_linear_speed), Vector4BaseFloat.Add(SteeringBehaviours.Cohesion(nearby, this, currentPosition, velocity, max_linear_speed, CohesionRadius), SteeringBehaviours.Flee(g, SBC.targetPosition, currentPosition, velocity, max_linear_speed, FOV))));
					break;
				case SB.CA:
					if (true) //weightedSum
					{
						steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), .2f);
						steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), .1f);
					}
					//else
					//{
					//    steerForce = Vector4BaseFloat.Add(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed));
					//}
					break;
				case SB.CAS:
					if (true) //weightedSum
					{
						steerForce = Vector4BaseFloat.Add(SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), Vector4BaseFloat.Add(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed)));
					}
					//else
					//{
					//    //steerForce = Vector4BaseFloat.Multiply(SteeringBehaviours.Flee(g, SBC.targetPosition, currentPosition, velocity, max_linear_speed, FOV), .4f);
					//    steerForce = Vector4BaseFloat.Multiply(SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), .3f);
					//    steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), .2f);
					//    steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), .5f);
					//}
					break;
				case SB.CS:
					if (true) //weightedSum
					{
						steerForce = Vector4BaseFloat.Add(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed));
					}
					//else
					//{
					//    steerForce = Vector4BaseFloat.Multiply(SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), .8f);
					//    steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), .2f);
					//}
					break;
				case SB.FCAS:
					if (true) //weightedSum
					{
						steerForce = Vector4BaseFloat.Add(SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), Vector4BaseFloat.Add(SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), Vector4BaseFloat.Add(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), SteeringBehaviours.Flee(targetPosition, currentPosition, velocity, targetPosition, max_linear_speed, FOV, 1))));
					}
					//else
					//{
					//    steerForce = Vector4BaseFloat.Multiply(SteeringBehaviours.Flee(targetPosition, currentPosition, velocity, targetPosition, max_linear_speed, FOV, 1), .4f);
					//    steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), .3f);
					//    steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), .2f);
					//    steerForce += Vector4BaseFloat.Multiply(SteeringBehaviours.Alignment(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), .5f);
					//}
					break;
				case SB.FCS:
					steerForce = Vector4BaseFloat.Add(SteeringBehaviours.Separation(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed), Vector4BaseFloat.Add(SteeringBehaviours.Cohesion(nearby, posAndSpeed, currentPosition, velocity, max_linear_speed, CohesionRadius), SteeringBehaviours.Flee(targetPosition, currentPosition, velocity, targetPosition, max_linear_speed, FOV, 1)));
					break;
				default:
					break;
			}
			steerForce = Vector4BaseFloat.Truncate3D(steerForce, max_force);
			acceleration = steerForce / mass;
			//velocity = velocity + acceleration;
			velocity.Set(Vector4BaseFloat.Truncate3D((velocity as Vector4BaseFloat) + acceleration, max_linear_speed));
			newPS.Position.Set(Vector4BaseFloat.Add((velocity as Vector4BaseFloat), currentPosition));
			return newPS;
		}

	}
}
